/*
 * RungeKuttaIntegrator.cpp
 *
 *  Created on: 20/05/2010
 *      Author: jorge
 */

#include "RungeKuttaIntegrator.h"
#include "../geometry/Vector3D.h"
using namespace geometry;

namespace physics {

RungeKuttaIntegrator::RungeKuttaIntegrator(float dt) : Integrator(dt) {

}

RungeKuttaIntegrator::~RungeKuttaIntegrator() {

}

void RungeKuttaIntegrator::integrate(Particle& p) {
	Vector3D gravity(0, -9.8, 0);

	Vector3D k1, l1, k2, l2, k3, l3, k4, l4;

	k1 = gravity;
	l1 = p.getVelocity();
	k2 = gravity;
	l2 = p.getVelocity() + (dt / 2.0) * k1;
	k3 = gravity;
	l3 = p.getVelocity() + (dt / 2.0) * k2;
	k4 = gravity;
	l4 = p.getVelocity() + dt * k3;

	p.addPosition((dt / 6.0) * (l1 + 2.0 * (l2 + l3) + l4));
	p.addVelocity((dt / 6.0) * (k1 + 2.0 * (k2 + k3) + k4));
}
}
